
#ifndef DYNAMIXEL_HAL_H_
#define DYNAMIXEL_HAL_H_

#include "system.h"
#include "stdint.h"
#include "string.h"
#include "usart.h"

class InstructionReceiver
{
  public:
    InstructionReceiver(void)
    {
      state = IDLE;
    }

    void start(void)
    {
      state = RECEIVING;
      buffer.clear();
    }

    void appendChar(char c)
    {
      if (state != RECEIVING) {
        return;
      }
      if (buffer.size() < 2 && c != 0xFF) {
        buffer.clear();
        return;
      }
      buffer += c;
      if (buffer.size() == 2) {
        int a;
        a = 1;
      }
      if (buffer.size() == 3) {
        int a;
        a = 1;
      }
      if (buffer.size() == 4) {
        data_length = buffer[3];
      }
      if (buffer.size() >= 6 && 
          buffer.size() == data_length + 4) {
        // Data package received, check checksum
        char checksum = 0;
        for (int idx = 2; idx < buffer.size(); idx++) {
          checksum += buffer[idx];
        }
        checksum = ~checksum;
        if (checksum == 0) {
          state = VALID;
        } else {
          state = INVALID;
        }
      }
    }

    bool isReceiving(void) {
      return state == RECEIVING;
    }
  
    enum {
      IDLE = 0,
      RECEIVING = 1,
      VALID = 2,
      INVALID = 3
    } state;
  
    static const int buffer_size = 160;
    String<buffer_size> buffer;
    int data_length;
};

class InstructionPacket
{
  public:
    InstructionPacket(void)
    {
    }

    InstructionPacket(int id, int instruction)
    {
      buffer.clear();
      buffer += (char) 0xFF;
      buffer += (char) 0xFF;
      buffer += (char) id;
      buffer += (char) 0x00;  // Place holder for length
      buffer += (char) instruction;
    }
  
    void append(char value)
    {
      buffer += (char) value;
    }

    void finalize(void)
    {
      buffer[3] = (char) (buffer.size() - 3);
      char checksum = 0;
      for (int idx = 2; idx < buffer.size(); idx++) {
        checksum += buffer[idx];
      }
      checksum = ~checksum;
      buffer += checksum;
    }

    bool busy(void)
    {
      return false;
    }

    enum {
      PING = 1,
      READ = 2,
      WRITE = 3,
      REG_WRITE = 4,
      ACTION = 5,
      RESET = 6,
      SYNC_WRITE = 131
    };
  
    static const int buffer_size = 160;
    String<buffer_size> buffer;
};

class DynamixelHal : public UsartInterrupt
{
  public:
    DynamixelHal(void);

    void work(uint32_t time);
    bool writeByte(int id, int address, int value);
    bool readByte(int id, int address, int *value);
    bool writeWord(int id, int address, int value);
    bool readWord(int id, int address, int *value);

    // Callback from USART
    void usartInterrupt(char data);

  private:
    bool usart_busy;
    Usart<SERVO_USART> usart;
    InstructionPacket instruction_packet;
    InstructionReceiver instruction_receiver;

};


#endif /* DYNAMIXEL_HAL_H_ */
